import cv2
import math
import numpy as np
import global_var as glv

resize_width = glv.getVal("resize_width")
resize_height = glv.getVal("resize_height")
color_range = glv.getColorRange(-1)
selfPoint = glv.getVal("selfPoint")
black = glv.black
######取得外界矩形坐标信息#############
def getCoordinate(rect):

    #根据y坐标降序排序
    sortIdex = np.lexsort([-1 * rect[:,1]])
    #前两个为下边界左右两点坐标
    bottom_left = rect[sortIdex[0]]
    bottom_right = rect[sortIdex[1]]
    down_k = (bottom_left[1] - bottom_right[1])/(bottom_left[0] - bottom_right[0])
    down_y = (bottom_left[1]+bottom_right[1])/2
    center_x = (bottom_left[0] + bottom_right[0])/2
    #后两个为上边界两点坐标
    up_left = rect[sortIdex[2]]
    up_right = rect[sortIdex[3]]
    up_k = (up_left[1] - up_right[1])/(up_left[0] - up_right[0])
    up_y = (up_left[1]+up_right[1])/2

    #根据x坐标降序排序
    sortIdex = np.lexsort([-1 * rect[:,0]])
    #前两个为右边界上下两点坐标
    right_up = rect[sortIdex[0]]
    right_down = rect[sortIdex[1]]
    right_x = (right_up[0] + right_down[0])/2
    try:
        right_k = (right_up[1] - right_down[1])/(right_up[0] - right_down[0])
    except:
        right_k = 999
    #后两个为左边界上下两点坐标
    left_up = rect[sortIdex[2]]
    left_down = rect[sortIdex[3]]
    left_x = (left_up[0] + left_down[0])/2
    try:
        left_k = (left_up[1] - left_down[1])/(left_up[0] - left_down[0])
    except:
        left_k = 999
    linesList = {"down_y": down_y,
                 "down_k":down_k,
                 "up_y":up_y,
                 "up_k":up_k,
                 "right_x":right_x,
                 "right_k":right_k,
                 "left_x":left_x,
                 "left_k":left_k}
    
    return linesList, center_x
########################################
########返回最大的轮廓########
def getAreaMaxContour(contours) :
        contour_area_temp = 0
        contour_area_max = 0
        area_max_contour = None;

        for c in contours : #历遍所有轮廓
            contour_area_temp = math.fabs(cv2.contourArea(c)) #计算轮廓面积
            if contour_area_temp > contour_area_max :
                contour_area_max = contour_area_temp
                if contour_area_temp > 300:  #只有在面积大于300时，最大面积的轮廓才是有效的，以过滤干扰
                    area_max_contour = c

        return area_max_contour, contour_area_max
##############################

####根据输入的轮廓点数组画轮廓及外接矩形，并返回边界信息####
def drawContour(orgImage, contours, graphtype = 1):
    if orgImage is not None:
        for point in contours:
            cv2.circle(orgImage,(int(point[0][0]),int(point[0][1])),1,(0,0,255),1)
    if graphtype == 1:
        rect = cv2.minAreaRect(contours)
        rectBox = np.int0(cv2.boxPoints(rect))
        if orgImage is not None:
            cv2.drawContours(orgImage, [rectBox], -1, (0,255,0), 2)           
        return rectBox
    elif graphtype == 2:
        (center_x,center_y), radius = cv2.minEnclosingCircle(contours)
        circle = {"center_x":center_x,
                  "center_y":center_y,
                  "radius":radius
            }
        return circle
############################################################

###########################################################

def findLine(lines, findHorizontal, selfPoint):
    '''
    寻找目标物体边缘线
    '''
    left_lines = []
    right_lines = []
    horizontal_lines = []
    if lines is not None:
        for line in lines:
            for x1,y1,x2,y2 in line:
                #计算线段斜率
                if x1 == x2:
                    continue
                else:
                    k = (y2-y1)/(x2-x1)
                line_length = math.sqrt(math.pow(y2-y1,2)+math.pow(x2-x1,2)) 
                line = np.append(line,np.array([[line_length,k]]),1)
                #根据斜率正负判断属于左右及直角车道,±15°
                if k < -0.36:
                    left_lines.append(line)
                elif k > 0.36:
                    right_lines.append(line)
                elif k <= 0.36 and k >= -0.36:
                    horizontal_lines.append(line)

        left_line = nearestLine(left_lines) 
        right_line = nearestLine(right_lines)
        horizontal_line = nearestLine(horizontal_lines)
        lines = {"left":left_line,
                 "right":right_line,
                 "horizontal":horizontal_line}
        return lines
    else:
        lines = None  
        return lines
def drawLine(orgImage, lines, color=(0,255,0),thickness = 2):
    if len(lines) != 0:
        lines_x, lines_y, lines_w, max_y, min_y = Choose_line(lines, 30, 0.05) 
        line_k, line_b = np.polyfit(lines_x,lines_y,1,w = lines_w)
        max_x = int((max_y-line_b)/line_k)
        min_x = int((min_y-line_b)/line_k)
        if max_x < 0:
            max_x = 0
        if min_x < 0:
            min_x = 0
        if max_x > resize_width:
            max_x = resize_width
        if min_x > resize_width:
            min_x = resize_width
        #画出左车道线
        cv2.line(orgImage, (min_x,int(min_y)),(max_x,int(max_y)),color,thickness)
        line = {"p1":(max_x,max_y),
                "p2":(min_x,min_y),
                "k":line_k,
                "b":line_b,
                "center_x":(max_x+min_x)/2,
                "center_y":(max_y+min_y)/2
            }
        return line
    else:
        return None
def nearestLine(lines):
    '''
    选择离point最近的直线
    '''
    global selfPoint
    outLine = None
    w = 0
    for line in lines:
        for x1,y1,x2,y2,l,k in line:
            b = y1 - k*x1
            son = math.fabs(k*selfPoint[0] - selfPoint[1] + l + b)
            mot = math.sqrt(math.pow(k,2)+1)
            distTemp = son/mot
            wTemp = distTemp * 0.4 + l * 0.6
            if wTemp > w:
                outLine = line
                w = wTemp
    if outLine is not None:
        for x1,y1,x2,y2,l,k in outLine:
            a = 180/math.pi * math.atan(k)
            outLine = {"p1":(int(x1),int(y1)),
                                "p2":(int(x2),int(y2)),
                                "k":k,
                                "b":b,
                                "a":a}
    return outLine
def getColor(hsvImage, minArea = 1000):
    global color_range
    areaMax = 0
    MaxColor = None
    for i in color_range:
        frame = cv2.inRange(hsvImage, color_range[i][0],color_range[i][1])
        opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((7,7),np.uint8))#开运算
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((7,7),np.uint8))#闭运算
        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)#找出轮廓
        _, areaMaxTemp = getAreaMaxContour(contours)#找出最大轮廓
        if areaMaxTemp > areaMax:
            areaMax = areaMaxTemp
            MaxColor = i
    print(areaMax)
    if areaMax > minArea:
        return MaxColor
    else:
        return None
def getLineByColor(hsvImage, targetColor, findHorizontal = False, outImage = None):
    global color_range
    global black
    global selfPoint
    left_line = None
    right_line = None
    horizontal_line = None
    if targetColor == "black":
        frame = cv2.inRange(hsvImage, black[0],black[1])
    else:
        frame = cv2.inRange(hsvImage, color_range[targetColor][0],color_range[targetColor][1])
    opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((7,7),np.uint8))#开运算
    closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((7,7),np.uint8))#闭运算
    canny = cv2.Canny(closed,50,150)
    cannyclosed = cv2.morphologyEx(canny, cv2.MORPH_CLOSE, np.ones((5,5),np.uint8))#闭运算
    lines = cv2.HoughLinesP(cannyclosed, 1,np.pi/180, 30, minLineLength = 40, maxLineGap = 30)
    lines = findLine(lines,findHorizontal, selfPoint)
    if lines is not None:
        if outImage is not None:
            if left_line is not None:
                cv2.line(outImage, left_line["p1"],left_line["p2"],(0,255,0),1)
            if right_line is not None:
                cv2.line(outImage, right_line["p1"],right_line["p2"],(0,255,0),1)
            if horizontal_line is not None:
                cv2.line(outImage, horizontal_line["p1"],horizontal_line["p2"],(0,255,0),1)
        return True, lines
    else:
        return False, None
#######################################
######根据输入颜色取得图像中颜色块轮廓#####
def getAreaByColor(hsvImage, targetColor, outframe = None, graphtype = 1, minArea = 0):
    global color_range
    global resize_height
    areaMaxContour = None
    areaMax = 0
    #红色有两个范围，需要分别处理
    if targetColor == "red":
        red = ["red_low","red_high"]
        for i in red:
            frame = cv2.inRange(hsvImage, color_range[i][0],color_range[i][1])
            opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((7,7),np.uint8))#开运算
            closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((7,7),np.uint8))#闭运算
            (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)#找出轮廓
            areaMaxContourTemp, areaMaxTemp = getAreaMaxContour(contours)#找出最大轮廓
            if areaMaxTemp > areaMax:
                areaMax = areaMaxTemp
                areaMaxContour = areaMaxContourTemp
    else:
        if targetColor == "black":
            frame = cv2.inRange(hsvImage, black[0],black[1])
        else:
            frame = cv2.inRange(hsvImage, color_range[targetColor][0],color_range[targetColor][1])
        opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((7,7),np.uint8))#开运算
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((7,7),np.uint8))#闭运算
        cv2.imshow("area", closed)
        cv2.waitKey(1)
        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)#找出轮廓
        areaMaxContour, areaMax = getAreaMaxContour(contours)#找出最大轮廓
    if graphtype == 1:
        if areaMaxContour is not None and areaMax > minArea:
            colorRect = drawContour(outframe, areaMaxContour)
            lineList, center_x = getCoordinate(colorRect)
            if outframe is not None:
                cv2.line(outframe,(int(center_x),0),(int(center_x), resize_height),(255,0,0),1)
            return True, lineList, center_x
        else:
            return False, None, None
    elif graphtype == 2:
        if areaMaxContour is not None and areaMax > minArea:
            circle = drawContour(outframe, areaMaxContour,2)
            if outframe is not None:
                cv2.circle(outframe,(int(circle["center_x"]),int(circle["center_y"])),int(circle["radius"]),(0,255,0),1)
                cv2.line(outframe,(int(circle["center_x"]),0),(int(circle["center_x"]), resize_height),(255,0,0),1)
            return True, circle
        else:
            return False, None
#########################################
